from LKMotor import LK_MotorControl as LK
from PyQt5.QtWidgets import *
from PyQt5.QtGui import *
from PyQt5.QtCore import QThread
from PyQt5.QtCore import QStringListModel
import time,sys
from JsonProg import *


sys.path.append('UI')
from Ui_UNV_PyController import *

class jointCtrl():
    '''
    集中各关节界面控制的相关控件、类和ID，方便使用循环遍历
    ctrlInstance: 电机控制实例
    tbCMDPos：目标位置文本框
    tbActPos: 实际位置文本框
    tbCanID: CANID文本框
    rbJogSel: 点动单选按钮

    '''
    ctrlInstance=0
    tbCmdPos=0
    tbActPos=0
    tbCanID=0
    rbJogSel=0
    CanID=0


class Config():
    '''
    用于保存界面设置，例如各关节对应CANID
    '''

    configDict={}
    configDict['canID']=[0]*6

    def __init__(self,_name=None):

        if _name==None:
            filename=self.__class__.__name__+".json"       
        else:
            filename=_name

        try:
            with open(filename) as infile:
                try:
                    self.configDict = json.load(infile)
                    print(self.posDict)
                except:
                    pass
        except:
            pass

    def saveConfig(self,_name=None):
        if _name==None:
            filename=self.__class__.__name__+".json"
        else:
            filename=_name
        with open(filename, "w") as outfile:
            json.dump(self.configDict, outfile)


class UNV_PyCtrllerUI(QMainWindow,Ui_MainWindow):
    '''
    主界面程序
    '''

    JointNo=6
    #joint_CANID=[0]*JointNo
    Joints=[0]*JointNo
    connected=0
    

    def __init__(self, parent=None):
        super(UNV_PyCtrllerUI,self).__init__(parent)
        self.setupUi(self)
        #self.txCmdPos_J1.setValidator(QDoubleValidator(-32767.0,32767.0,3))
        self.txDelayTime.setValidator(QIntValidator(0,10000))
        self.btnMove.clicked.connect(self.runMotor)
        self.btnStop.clicked.connect(self.stopMotor)
        self.btnRdStatus.clicked.connect(self.rdStatus)
        self.btnRdStatus.setText("开始\n读取状态")
        self.btnConnect.clicked.connect(self.connect)
        self.btnDisconnect.setEnabled(False)
        self.btnDisconnect.clicked.connect(self.disconnect)
        self.btnSaveCurrentPos.clicked.connect(self.saveCurrentPos)
        self.btnSaveCmdPos.clicked.connect(self.saveCmdPos)
        self.btnMenuJog.clicked.connect(lambda: self.switchStackedPage(0))
        self.btnMenuMdi.clicked.connect(lambda: self.switchStackedPage(1))
        self.btnMenuProg.clicked.connect(lambda: self.switchStackedPage(2))
        self.btnMenuConfig.clicked.connect(lambda: self.switchStackedPage(3))
        self.btnJogPlus.pressed.connect(self.jogPlusPress)
        self.btnJogPlus.released.connect(self.jogPlusRelease)
        self.btnJogNeg.pressed.connect(self.jogNegPress)
        self.btnJogNeg.released.connect(self.jogNegRelease)
        self.btnDelSelPos.clicked.connect(self.mdiDelSelPos)
        self.btnLoadSelPos.clicked.connect(self.mdiLoadSelPos)
        self.btnAddDelay.clicked.connect(lambda: self.addProgBlock_Delay(int(self.txDelayTime.text()),self.txDelayName.text()))
        self.btnAddWaitP.clicked.connect(lambda: self.addProgBlock_WaitP(self.txWaitPName.text()))
        self.btnAddPTP.clicked.connect(lambda: self.addProgBlock_PTP(self.txPTPName.text(),self.cbPTPPos.currentText()))
        self.btnSaveProg.clicked.connect(self.saveProg)
        self.btnLoadProg.clicked.connect(self.loadProg)
        


        #self.btnConnect.setText("断开连接")

        #print(Ui_MainWindow.__dict__)
        for i in range(self.JointNo):
            self.Joints[i]=jointCtrl()

        self.Joints[0].tbActPos=self.txActPos_J1
        self.Joints[0].tbCmdPos=self.txCmdPos_J1
        self.Joints[0].tbCanID=self.txCANID_J1
        self.Joints[0].rbJogSel=self.rbJogJ1
        self.Joints[1].tbActPos=self.txActPos_J2
        self.Joints[1].tbCmdPos=self.txCmdPos_J2
        self.Joints[1].tbCanID=self.txCANID_J2
        self.Joints[1].rbJogSel=self.rbJogJ2
        self.Joints[2].tbActPos=self.txActPos_J3
        self.Joints[2].tbCmdPos=self.txCmdPos_J3
        self.Joints[2].tbCanID=self.txCANID_J3   
        self.Joints[2].rbJogSel=self.rbJogJ3    
        self.Joints[3].tbActPos=self.txActPos_J4
        self.Joints[3].tbCmdPos=self.txCmdPos_J4
        self.Joints[3].tbCanID=self.txCANID_J4
        self.Joints[3].rbJogSel=self.rbJogJ4
        self.Joints[4].tbActPos=self.txActPos_J5
        self.Joints[4].tbCmdPos=self.txCmdPos_J5
        self.Joints[4].tbCanID=self.txCANID_J5
        self.Joints[4].rbJogSel=self.rbJogJ5
        self.Joints[5].tbActPos=self.txActPos_J6
        self.Joints[5].tbCmdPos=self.txCmdPos_J6
        self.Joints[5].tbCanID=self.txCANID_J6
        self.Joints[5].rbJogSel=self.rbJogJ6
        for i in range(self.JointNo):
            #self.Joints[i].tbActPos.setValidator(QDoubleValidator(-32767000.0,32767000.0,3))
            self.Joints[i].tbCmdPos.setValidator(QDoubleValidator(-32767000.0,32767000.0,3))
            self.Joints[i].tbCanID.setValidator(QIntValidator())

            # self.Joints[i].tbActPos=vars(UNV_PyCtrllerUI)["self.txActPos_J"+str(i)]
            # self.Joints[i].tbActPos.setValidator(QDoubleValidator(-32767000.0,32767000.0,3))
            # self.Joints[i].tbCmdPos=locals()["self.txCmdPos_J"+str(i)]
            # self.Joints[i].tbCmdPos.setValidator(QDoubleValidator(-32767000.0,32767000.0,3))
            # self.Joints[i].tbCanID=locals()["self.txCANID_J"+str(i)]
            # self.Joints[i].tbCanID.setValidator(QIntValidator())
        self.config=Config()
        for i in range(self.JointNo):
            self.Joints[i].tbCanID.setText(str(self.config.configDict['canID'][i]))
        self.posList=jointPos()
        self.posList.loadPos()
        self.cbPTPPos.addItems(self.posList.posDict.keys())
        self.setupPosListView()
            
        self.prog1=CreateProgram("prog1")

        #self.J1_Ctrl=LK(1)
        #J1_Ctrl.GetMultiturnAngle()
        #self.txActPos_J1.setText(str(J1_Ctrl.motorParam.motorAngle)) 
        self.rdActPosThread=rdActPosThread(self.Joints)

    
    def runMotor(self):
        self.rdActPosThread.rdPosStart=0
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                CmdPos=float(self.Joints[i].tbCmdPos.text())
                self.Joints[i].ctrlInstance.Ctrl_MultiturnAngle(int(CmdPos))
                time.sleep(0.005)
        #CmdPos=float(self.txCmdPos_J1.text())
        #J1_Ctrl.Ctrl_MultiturnAngle(int(CmdPos))
        self.rdActPosThread.rdPosStart=1
        #self.J1_Ctrl.GetMultiturnAngle()
        #self.txActPos_J1.setText(str(self.J1_Ctrl.motorParam.motorAngle)) 

    def stopMotor(self):
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                self.Joints[i].ctrlInstance.Ctrl_MotorStop()
                time.sleep(0.005)
            

        
    def rdStatus(self):
        #J1_Ctrl.GetMultiturnAngle()
        #self.txActPos_J1.setText(str(J1_Ctrl.motorParam.motorAngle)) 
        if self.rdActPosThread.rdPosStart==0:
            self.rdActPosThread.start()
            self.rdActPosThread.rdPosStart=1
            self.btnRdStatus.setText("停止\n读取状态")
        else:
            self.rdActPosThread.terminate()
            self.rdActPosThread.rdPosStart=0
            self.btnRdStatus.setText("开始\n读取状态")

    def connect(self):

        for i in range(self.JointNo):
            self.Joints[i].CanID=0

        for i in range(self.JointNo):
            _canID=int(self.Joints[i].tbCanID.text())
            self.Joints[i].CanID=_canID
            self.config.configDict['canID'][i]=_canID
            self.Joints[i].ctrlInstance=LK(_canID)
            time.sleep(0.005)
            
        self.config.saveConfig()
        self.connected=1
        self.btnDisconnect.setEnabled(True)
        self.btnConnect.setEnabled(False)
        self.frmCanID.setEnabled(False)


        # if J1_Ctrl.connected:
        #     J1_Ctrl.disconnect()
        #     self.btnConnect.setText("连接")
        # else:
        #     J1_Ctrl.__init__(1)
        #     self.btnConnect.setText("断开连接")
    def disconnect(self):
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                self.Joints[i].ctrlInstance.disconnect()
        self.connected=0
        self.btnDisconnect.setEnabled(False)
        self.btnConnect.setEnabled(True)
        self.frmCanID.setEnabled(True)

    def saveCurrentPos(self):
        tempPos=[]
        for i in range(self.JointNo):
            tempPos.append(float(self.Joints[i].tbActPos.text()))
        self.posList.addPos(tempPos)
        self.setupPosListView()
        self.posList.savePos()
        #print(self.posList.posDict)

    def saveCmdPos(self):
        tempPos=[]
        for i in range(self.JointNo):
            tempPos.append(float(self.Joints[i].tbCmdPos.text()))
        self.posList.addPos(tempPos)
        self.setupPosListView()
        self.posList.savePos()
        self.cbPTPPos.addItems(self.posList.posDict.keys())
        #print(self.posList.posDict)

    def setupPosListView(self):
        '''
        更新储存位置列表ListView
        '''
        slm = QStringListModel()
        slm.setStringList(self.posList.posDict.keys())
        self.listPos.setModel(slm)

    def switchStackedPage(self,_index):
        self.stackedWidget.setCurrentIndex(_index)

    def jogPlusPress(self):
        '''
        点动+方向按下
        '''
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                if self.Joints[i].rbJogSel.isChecked():
                    self.Joints[i].ctrlInstance.Ctrl_Speed(self.slJogSpeed.value()*1000)

    def jogPlusRelease(self):
        '''
        点动+方向松开
        '''
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                self.Joints[i].ctrlInstance.Ctrl_MotorStop()
                    

    def jogNegPress(self):
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                if self.Joints[i].rbJogSel.isChecked():
                    self.Joints[i].ctrlInstance.Ctrl_Speed(self.slJogSpeed.value()*-1000) 

    def jogNegRelease(self):
        for i in range(self.JointNo):
            if self.Joints[i].CanID>0:
                if self.Joints[i].rbJogSel.isChecked():
                    self.Joints[i].ctrlInstance.Ctrl_MotorStop() 

    def mdiLoadSelPos(self):
        selected = self.listPos.selectedIndexes()
        if selected:
            selectedPos=selected[0].data()
            for i in range(self.JointNo):
                self.Joints[i].tbCmdPos.setText(str(self.posList.posDict[selectedPos][i]))

        
    def mdiDelSelPos(self):
        selected = self.listPos.selectedIndexes()
        if selected:
            selectedPos=selected[0].data()
            del self.posList.posDict[selectedPos]
            self.setupPosListView()
            self.posList.savePos()

    def addProgBlock_PTP(self,_name,_pos):
        
        if _pos!="":
            if _name!="":
                self.prog1.addBlock(_name,blockType.PTP,self.posList.posDict[_pos])
            else:
                for i in range(self.prog1.maxBlockNo):
                    tempName="PTP_"+str(_pos)+"_"+str(i)
                    if not (tempName in set([block.name for block in self.prog1.progList])):
                        self.prog1.addBlock(tempName,blockType.PTP,self.posList.posDict[_pos])                   
                        break
        #print(self.prog1.progList[-1].param)
        self.setupProgListView()            

    def addProgBlock_WaitP(self,_name):
        if _name!="":
            self.prog1.addBlock(_name,blockType.WaitP)
        else:
            for i in range(self.prog1.maxBlockNo):
                tempName="WaitP_"+str(i)
                if not (tempName in set([block.name for block in self.prog1.progList])):
                    self.prog1.addBlock(tempName,blockType.WaitP)                   
                    break
        self.setupProgListView()

    def addProgBlock_Delay(self,_delay,_name):
        if _name!="":
            self.prog1.addBlock(_name,blockType.Delay,_delay)
        else:
            for i in range(self.prog1.maxBlockNo):
                tempName="Delay_"+str(_delay)+"ms_"+str(i)
                if not (tempName in set([block.name for block in self.prog1.progList])):
                    self.prog1.addBlock(tempName,blockType.Delay,_delay)                   
                    break
        self.setupProgListView()

    def delSelProgBlock(self):
        '''
        删除在程序段列表中选中的程序段
        '''
        pass

    def setupProgListView(self):
        '''
        更新程序段列表QListView
        '''
        slm = QStringListModel()
        #blocklist=[block.name for block in self.prog1.progList]
        #print(blocklist)
        slm.setStringList([block.name for block in self.prog1.progList])
        self.listProg.setModel(slm)

    def saveProg(self):
        '''
        保存程序
        TODO: 多程序管理
        '''
        self.prog1.saveProg()

    def loadProg(self):
        '''
        加载程序
        TODO: 多程序管理
        '''
        self.prog1.loadProg()
        self.setupProgListView()




class rdActPosThread(QThread):
    '''
    QTread用于读取电机实际位置并更新对应文本框
    '''
    rdPosStart=0
    JointNo=6
    def __init__(self,_joints):
        super(rdActPosThread, self).__init__()
        self.Joints=_joints

    def run(self):
        while self.rdPosStart:
            for i in range(self.JointNo):
                if self.Joints[i].CanID>0:
                    self.Joints[i].ctrlInstance.GetMultiturnAngle()
                    self.Joints[i].tbActPos.setText(str(self.Joints[i].ctrlInstance.motorParam.motorAngle))
                    time.sleep(0.005)
            #J1_Ctrl.GetMultiturnAngle()
            #self.textbox.setText(str(J1_Ctrl.motorParam.motorAngle)) 
            #print("ThreadRd",time.time())
            time.sleep(0.5)

if __name__ == '__main__':
    #J1_Ctrl=LK(1)
    app = QApplication(sys.argv)
    #MainWindow = QMainWindow()
    ui =UNV_PyCtrllerUI()
    #ui.setupUi(MainWindow)
    ui.show()
    sys.exit(app.exec_())
